#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtle_bt_ros2/msg/pose.hpp>
# include <turtle_bt_ros2/srv/spawn.hpp>

#include <memory>
#include <string>

using std::placeholders::_1;

class My_turtle_tf : public rclcpp::Node {
public:
    My_turtle_tf()
        : Node("My_turtle_pose1_subscriber_tf") {
        this->declare_parameter<std::string>("turtlename", "turtle1");
        this->get_parameter("turtlename", turtle_name);

        br = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

        std::ostringstream stream;
        stream << "/" << turtle_name.c_str() << "/pose";
        std::string topic_name = stream.str();

        subscription_ = this->create_subscription<turtle_bt_ros2::msg::Pose>(
                                        topic_name, 10, std::bind(&My_turtle_tf::topic_callback, this, _1));
    }

private:
    void topic_callback(const turtle_bt_ros2::msg::Pose::SharedPtr msg) {
        rclcpp::Time now;

        // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
        geometry_msgs::msg::TransformStamped transform;

        transform.transform.translation.x = msg->x;
        transform.transform.translation.y = msg->y;
        transform.transform.translation.z = 0.0;

        transform.header.stamp = now;
        transform.header.frame_id = "world";
        transform.child_frame_id = turtle_name;

        tf2::Quaternion q;
        q.setRPY(0, 0, msg->theta);
        transform.transform.rotation.w = q.w();
        transform.transform.rotation.x = q.x();
        transform.transform.rotation.y = q.y();
        transform.transform.rotation.z = q.z();

        // 发布坐标变换
        br->sendTransform(transform);
    }
    rclcpp::Subscription<turtle_bt_ros2::msg::Pose>::SharedPtr subscription_;
    std::unique_ptr<tf2_ros::TransformBroadcaster> br;
    std::string turtle_name;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<My_turtle_tf>());
    // rclcpp::shutdown();
    return 0;
}